package dk.controlme.lego.client;

import dk.controlme.lego.common.Action;
import lejos.robotics.subsumption.Behavior;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.navigation.SimpleNavigator;
import lejos.robotics.navigation.Pilot;
import lejos.nxt.LCD;
import lejos.nxt.Motor;

public class DoMove implements Behavior {

    /*==========================================================*/
    final float wheelTrackWidth = 114f; // Width between tracks
    final float wheelDiameter   = 15f;  // Diameter of wheels
    Motor leftMotor             = Motor.A;
    Motor rightMotor            = Motor.B;
    final int motorSpeed        = 180;  // Degrees/second
    /*==========================================================*/
    BluetoothConnection bluetoothConnection;
    private static SimpleNavigator navigator;


    public DoMove(BluetoothConnection bluetoothConnection) {
        Pilot pilot = new TachoPilot(wheelDiameter, wheelTrackWidth, leftMotor, rightMotor);
        DoMove.navigator = new SimpleNavigator(pilot);
        DoMove.navigator.setMoveSpeed(motorSpeed);
        this.bluetoothConnection = bluetoothConnection;
    }

    public boolean takeControl() {
        // We want control - always!
        // Remember DoExit has higher priority than us anyway..
        return true;
    }

    public void suppress() {
        // Nothing to suppress...
    }

    public void action() {
        if(bluetoothConnection.getNewCommand()) {
            Action action = Action.getActionById(bluetoothConnection.getCommand());

            // Make sure the drawString strings are EXACTLY the same length
            // otherwise bits from the previous strings will show on the display
            if ( action == Action.STOP ) {
                LCD.drawString("Stopping       ", 0, 1);
                LCD.refresh();
                DoMove.navigator.stop();
            } else if ( action == Action.LEFT ) {
                LCD.drawString("Turning left   ", 0, 1);
                LCD.refresh();
                DoMove.navigator.rotateLeft();
            } else if ( action == Action.RIGHT ) {
                LCD.drawString("Turning right  ", 0, 1);
                LCD.refresh();
                DoMove.navigator.rotateRight();
            } else if ( action == Action.FORWARD ) {
                LCD.drawString("Moving forward ", 0, 1);
                LCD.refresh();
                DoMove.navigator.forward();
            } else if ( action == Action.BACKWARD ) {
                LCD.drawString("Moving backward", 0, 1);
                LCD.refresh();
                DoMove.navigator.backward();
            } else if ( action == Action.EXIT ) {
                // Exit through bluetooth.
                LCD.drawString("Exiting        ", 0, 1);
                LCD.refresh();
                System.exit(0);
            }
        }
    }
}
